#include "livox_parser.h"
#include "cyber/common/log.h"

#include <cmath>
#include <cstring>

namespace apollo {
namespace drivers {
namespace livox {

bool LivoxParser::Init() { return true; };

void LivoxParser::ParseData(const LivoxPacket *data, PointCloud *pc) {
  if (pc == nullptr || data == nullptr) {
    AERROR << "Livox parser find empty data!";
    return;
  }
  pc->mutable_point()->Reserve(23'000);
  switch (data->data_type) {
    case kLivoxLidarCartesianCoordinateHighData:
      CartesianCoordinateHighDataProcess(data, pc);
      break;
    case kLivoxLidarCartesianCoordinateLowData:
      CartesianCoordinateLowDataProcess(data, pc);
      break;
    case kLivoxLidarSphericalCoordinateData:
      AERROR << "Sepherical Coordinate not supported, please use Livox "
                  "viewer to change it.";
    default:
      AERROR << "data_type is wrong: " << (int)data->data_type;
  }
}

void LivoxParser::CartesianCoordinateHighDataProcess(const LivoxPacket *data,
                                                     PointCloud *cloud) {
  uint16_t size = data->dot_num;
  LivoxLidarCartesianHighRawPoint *raw =
      (LivoxLidarCartesianHighRawPoint *)data->buf;
  for (uint32_t i = 0; i < size; ++i) {
    if ((raw[i].tag != 0 && raw[i].tag != 16) || (raw[i].x == 0 && raw[i].y == 0 && raw[i].z == 0)) continue;
    PointI *point_new = cloud->add_point();
    point_new->set_x(static_cast<float>(raw[i].x / 1000.0));
    point_new->set_y(static_cast<float>(raw[i].y / 1000.0));
    point_new->set_z(static_cast<float>(raw[i].z / 1000.0));
    point_new->set_intensity(raw[i].reflectivity);
    point_new->set_timestamp(GetStoragePacketTimestamp(data));
  }
}

void LivoxParser::CartesianCoordinateLowDataProcess(const LivoxPacket *data,
                                                    PointCloud *cloud) {
  uint16_t size = data->dot_num;
  const LivoxLidarCartesianLowRawPoint *raw =
      (const LivoxLidarCartesianLowRawPoint *)data->buf;
  // add new point cloud
  for (uint32_t i = 0; i < size; i++) {
    if ((raw[i].tag != 0 && raw[i].tag != 16) || (raw[i].x == 0 && raw[i].y == 0 && raw[i].z == 0)) continue;
    PointI *point_new = cloud->add_point();
    point_new->set_x(static_cast<float>(raw[i].x / 1000.0));
    point_new->set_y(static_cast<float>(raw[i].y / 1000.0));
    point_new->set_z(static_cast<float>(raw[i].z / 1000.0));
    point_new->set_intensity(raw[i].reflectivity);
    point_new->set_timestamp(GetStoragePacketTimestamp(data));
  }
}

uint64_t LivoxParser::GetStoragePacketTimestamp(const LivoxPacket *data) {
  uint64_t timestamp;
  memcpy(&timestamp, data->timestamp, sizeof(timestamp));
  if (data->time_type == 0) {
    // AINFO << "Livox not synced, check PTP status.";
    return timestamp;
  } else if (data->time_type == 1 || data->time_type == 2) {
    return timestamp;
  } else {
    // AERROR << "Livox time type not valid: time type is " <<
    // data->time_type;
    return 0;
  }
}

}  // namespace livox
}  // namespace drivers
}  // namespace apollo
